diff --git a/arch/arm/cpu/armv7/mx6/Kconfig b/arch/arm/cpu/armv7/mx6/Kconfig
index af6dad3..8066aaa 100644
--- a/arch/arm/cpu/armv7/mx6/Kconfig
+++ b/arch/arm/cpu/armv7/mx6/Kconfig
@@ -173,6 +173,7 @@
 
 config TARGET_MX6Q_ICORE
 	bool "Support Engicam i.Core"
+	select BOARD_LATE_INIT
 	select MX6QDL
 	select OF_CONTROL
 	select DM
diff --git a/board/engicam/icorem6/icorem6.c b/board/engicam/icorem6/icorem6.c
index 171ec45..f78f8c9 100644
--- a/board/engicam/icorem6/icorem6.c
+++ b/board/engicam/icorem6/icorem6.c
@@ -205,6 +205,25 @@
 	return 0;
 }
 
+int board_late_init(void)
+{
+	switch ((imx6_src_get_boot_mode() & IMX6_BMODE_MASK) >>
+			IMX6_BMODE_SHIFT) {
+	case IMX6_BMODE_SD:
+	case IMX6_BMODE_ESD:
+		setenv("modeboot", "mmcboot");
+		break;
+	case IMX6_BMODE_NAND:
+		setenv("modeboot", "nandboot");
+		break;
+	default:
+		setenv("modeboot", "");
+		break;
+	}
+
+	return 0;
+}
+
 int board_init(void)
 {
 	/* Address of boot parameters */
diff --git a/include/configs/imx6qdl_icore.h b/include/configs/imx6qdl_icore.h
index 5a28b15..b517e87 100644
--- a/include/configs/imx6qdl_icore.h
+++ b/include/configs/imx6qdl_icore.h
@@ -64,7 +64,7 @@
 	"fitboot=echo Booting FIT image from mmc ...; " \
 		"run mmcargs; " \
 		"bootm ${loadaddr}\0" \
-	"mmcboot=echo Booting from mmc ...; " \
+	"_mmcboot=run mmcargs; " \
 		"run mmcargs; " \
 		"if test ${boot_fdt} = yes || test ${boot_fdt} = try; then " \
 			"if run loadfdt; then " \
@@ -79,6 +79,20 @@
 		"else " \
 			"bootm; " \
 		"fi\0" \
+	"mmcboot=echo Booting from mmc ...; " \
+		"if mmc rescan; then " \
+			"if run loadbootscript; then " \
+				"run bootscript; " \
+			"else " \
+				"if run loadfit; then " \
+					"run fitboot; " \
+				"else " \
+					"if run loadimage; then " \
+						"run _mmcboot; " \
+					"fi; " \
+				"fi; " \
+			"fi; " \
+		"fi\0" \
 	"nandboot=echo Booting from nand ...; " \
 		"if mtdparts; then " \
 			"echo Starting nand boot ...; " \
@@ -90,25 +104,7 @@
 		"nand read ${fdt_addr} dtb 0x100000; " \
 		"bootm ${loadaddr} - ${fdt_addr}\0"
 
-#ifdef CONFIG_NAND_MXS
-# define CONFIG_BOOTCOMMAND		"run nandboot"
-#else
-# define CONFIG_BOOTCOMMAND \
-	"mmc dev ${mmcdev};" \
-	"if mmc rescan; then " \
-		"if run loadbootscript; then " \
-			"run bootscript; " \
-		"else " \
-			"if run loadfit; then " \
-				"run fitboot; " \
-			"else " \
-				"if run loadimage; then " \
-					"run mmcboot; " \
-				"fi; " \
-			"fi; " \
-		"fi; " \
-	"fi"
-#endif
+#define CONFIG_BOOTCOMMAND		"run $modeboot"
 
 /* Miscellaneous configurable options */
 #define CONFIG_SYS_MEMTEST_START	0x80000000